import * as math from "mathjs";

function d2r(d){
  return d * Math.PI / 180;
}

function r2d(r){
  return r * 180 / Math.PI;
}

function rpy2rv( roll,pitch,yaw ){
  
  const alpha = d2r(yaw)
  const beta = d2r(pitch)
  const gamma = d2r(roll)

  const ca = Math.cos(alpha);
  const cb = Math.cos(beta);
  const cg = Math.cos(gamma);
  const sa = Math.sin(alpha);
  const sb = Math.sin(beta);
  const sg = Math.sin(gamma);

  const r11 = ca*cb;
  const r12 = ca*sb*sg-sa*cg;
  const r13 = ca*sb*cg+sa*sg;
  const r21 = sa*cb;
  const r22 = sa*sb*sg+ca*cg;
  const r23 = sa*sb*cg-ca*sg;
  const r31 = -sb;
  const r32 = cb*sg;
  const r33 = cb*cg;

  const theta = Math.acos((r11+r22+r33-1)/2);
  const sth = Math.sin(theta);
  const kx = (r32-r23)/(2*sth);
  const ky = (r13-r31)/(2*sth);
  const kz = (r21-r12)/(2*sth);

  return [(theta*kx),(theta*ky),(theta*kz)]
}

function rv2rpy(rx,ry,rz){

  const theta = Math.sqrt(rx*rx + ry*ry + rz*rz);
  const kx = rx/theta;
  const ky = ry/theta;
  const kz = rz/theta;
  const cth = Math.cos(theta);
  const sth = Math.sin(theta);
  const vth = 1-Math.cos(theta);

  const r11 = kx*kx*vth + cth;
  const r12 = kx*ky*vth - kz*sth;
  const r13 = kx*kz*vth + ky*sth;
  const r21 = kx*ky*vth + kz*sth;
  const r22 = ky*ky*vth + cth;
  const r23 = ky*kz*vth - kx*sth;
  const r31 = kx*kz*vth - ky*sth;
  const r32 = ky*kz*vth + kx*sth;
  const r33 = kz*kz*vth + cth;

  let alpha, beta, gamma;
  beta = Math.atan2(-r31,Math.sqrt(r11*r11+r21*r21))

  if (beta > d2r(89.99)){
    beta = d2r(89.99)
    alpha = 0
    gamma = Math.atan2(r12,r22)
  }
  else if (beta < -d2r(89.99)){
    beta = -d2r(89.99)
    alpha = 0
    gamma = -Math.atan2(r12,r22)
  }
  else{
    const cb = Math.cos(beta)
    alpha = Math.atan2(r21/cb,r11/cb)
    gamma = Math.atan2(r32/cb,r33/cb)
  }

  return [r2d(gamma),r2d(beta),r2d(alpha)]
}

function printMatrix( value ) {
  const precision = 5;
  console.log(math.format(value, precision));
}

function getTMatrix( pose ) {
  
  if( pose.length !== 6 ) return;
  
  const x = pose[0];
  const y = pose[1];
  const z = pose[2];
  const rx = pose[3];
  const ry = pose[4];
  const rz = pose[5];

  const theta = Math.sqrt(rx*rx + ry*ry + rz*rz);
  const kx = rx/theta;
  const ky = ry/theta;
  const kz = rz/theta;
  const cth = Math.cos(theta);
  const sth = Math.sin(theta);
  const vth = 1-Math.cos(theta);

  const r11 = kx*kx*vth + cth;
  const r12 = kx*ky*vth - kz*sth;
  const r13 = kx*kz*vth + ky*sth;
  const r21 = kx*ky*vth + kz*sth;
  const r22 = ky*ky*vth + cth;
  const r23 = ky*kz*vth - kx*sth;
  const r31 = kx*kz*vth - ky*sth;
  const r32 = ky*kz*vth + kx*sth;
  const r33 = kz*kz*vth + cth;

  const mat = [
    [r11, r12, r13, x],
    [r21, r22, r23, y],
    [r31, r32, r33, z],
    [0,   0,   0,   1],
  ]

  return mat;
}

export {
  printMatrix,
  d2r,
  r2d,
  rpy2rv,
  rv2rpy,
  getTMatrix,
}